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ABSTRACT 

The robotics lab at the Kennedy Space Center 

= 

describes a new approach to^wU.ng 'h^om simult aneously avoiding 

* — 

demonstrate the efficacy of the control law. 
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L INTRODUCTION 


The range of motion achievable by a robot manipulator's end effector is a function 
of the number and type of joints or degrees of freedom it possesses. Any degrees of 
freedom in excess of the minimum number required to reach an arbitrary end effector 
position and orientation within the workspace are considered "redundant". Commercial 
manipulators typically possess six or fewer DOF for primarily "anthropomorphic tasks 
such as industrial assembly and are therefore not redundant. 


There are some tasks for which such standard manipulators are not well suited, 
such as those requiring an extended reach in a confined workspace. For that reason, so- 
called "serpentine" manipulators have attracted interest Their designation and appearance 
(Fig. 1) suggest the long reach and dexterity associated with snakes or tentacles. ey 
achieve this snake-like ability by possessing a high degree of redundancy. This 

redundancy allows them, theoretically, to "wriggle" an end effector into a confined or 
difficult to reach point while allowing the robot arm to be configured in such a way 
not contact the surrounding environment. 



Figure 1 

Serpentine Manipulator 
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One possible application is the inspection and processing of shuttle orbiter 
payloads in the Payload Changeout Room. During final launch preparations, tasks such as 
connecting/disconnecting umbilicals, removal of lens covers, or visual inspections must be 
carried out on Shuttle payloads. It is difficult and sometimes treacherous for technicians 
to see or reach many of the points at which these processes must take place and the 
payload itself may be put at risk. A serpentine robot is currently under development at 
KSC to study the feasibility for its use for such tasks [1], 

There are two traditional approaches to controlling robot motion: to determine 
the dynamical equations of motion for each of the joints and generating the required 
torque for desired end-effector motion, or to control the joint velocities in response to the 
robot kinematics. 

The complexity of serpentine motion, coupled with collision avoidance 
requirements, typically dictate relatively slow motion. This usually renders the dynamics 
of the robot arm negligible. Therefore, only the kinematics of the serpentine motion need 
be addressed. 

Several approaches to controlling redundant manipulators for collision avoidance 
have been suggested. Maciejewski and Klein [2], Nakamura [3] and Wegerif, et al [4] 
make use of the pseudo-inverse and some variations of null-motion. Sciavicco, and 
Siciliano [5] make use of Lyapunov stability and an augmented configuration space to 
track a prescribed trajectory and incorporate obstacle avoidance. Alternatively, Pasch 
[1], and Asano [6] prescribe an end effector path and cause each joint to follow it in a 
"follow-the-leader' mode. All of these methods require that at least the end effector’s 
trajectory and velocity be prescribed. This presumes that a clear path for the end effector 
is easily determined. Only [4] allows for the end effector to deviate from the prescribed 
path but only as an emergency measure. 

In this paper, the author presents an alternative method for determining an 
acceptable robot trajectory which allows the end effector's path, as well as the entrained 
link’s to be free to move around obstacles. 
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H. MANIPULATOR KINEMATICS 


The position and orientation of the end effector re iT is described, using the 
standard Denavitt-Hartenberg convention as a function of the generalize vector o joi 

displacements q e R H for a manipulator with n joints. 

r = r(q(t),t) <0 

ax 

frame is given by: 


-}R = 


cosd i -sin0,cosa ( 
sinOj COS0, cos a, 
0 sin a, 


sin 0, sin a, 
-cosO, sin a, , 
cos a, 


( 2 ) 


The angle a. reflects the rotation of the i, h joint frame about the local x axts wtth respect 
to the (M)* frame. The angle 0, is the rotation angle of the i, h jomt and correspon s 
t0 a component of the vector , . The end of the link is located by the vector 

o/ 


0 


( 3 ) 

(4) 


r_< = Li- \ + ''l R l l > 

so that the end effector is located by: 

Serpentine manipulttms^tre jQ^^^ssentiall^ u^versa^ jo^n^posseKing^wo 

fenTFlo^The^convention. the link frames are alternately rotated about the 
ISf« axes with every other link having zero length. Equatton (2) becomes. 


a. = 




cosfy 

0 

- sin 0, 


COS0, 

0 

sin0 i 

f-/r/2, i 

even ^^= 

sin 0, 

0 

cosfy 

even n 

> odd 1 ^ = 

sin 0, 

0 

-COS0, 

{ it/ 2, i 

odd 

0 

-1 

0 


0 

1 

0 


(5) 


The location of the link ends and the end effe ctor .then found by rf five ^ 

for simulation pu^oses a nouona^ that „„ translate in the base frame x 

and their graphical presentatron were fachtated by 
Mathematica [8] , a symbolic mathematics software package. 
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m. INVERSE KINEMATICS 

The velocity of the end effector is calculated by : 


dr_ 

dt 


r dr dq 


( 6 ) 


where the coefficient matrix ,{dr_ / dq) — J , is the Jacobian matrix. 

To achieve a desired trajectory for the end effector, when n>3, the differential equation 
for the generalized joint displacements may be solved: 

q=J*r ( 7 ) 

where J* is the Moore-Penrose pseudo inverse given by: 

= ( 8 ) 

Eq ( 7 ) yields the minimum norm solution for £. This solution assumes an appropriate 

scaling metric as discussed by Doty, et al [9]. In fact, because in a redundant manipulator, 
n>m, there are infinitely many solutions to Eq. ( 6 ). The joint rates are a function of those 

rates which cause the end effector to move, q R and "null rates", q s which do not. That 

is, q = q +q where q R is the minimum norm motion given by Eq. (7) and q v is given 
by: 

q_ N ={E n -J*J)li ( 9 ) 

where E n is the nxn identity matrix and \i&R n is an arbitrary vector. It is a simple 

matter to confirm that q and q are orthogonal vectors by taking their inner product. 
Alternatively, multiplying Eq. ( 8 ) by J yields the null vector. 

The selection of ji generates one of an infinite number of joint rate combinations 

which move all of the links but do not cause motion in the end effector. There have been 
several control laws suggested which make use of the null motion to avoid obstacles. 
Several of these are recapped by Nakamura [3] in some detail. A major shortcoming of 
these methods is the requirement to prescribe the end effector path and velocity. Not only 
can this be a difficult task in itself for a complex workspace, but, in some situations, it 
proscribes joint motion which could avoid collisions. 
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rv. LYAPUNOV STABILITY APPROACH TO END EFFECTOR TRAJECTORY 

In contrast to the pseudo-inverse approach, the author has adopted a Lyapunov 
stability measure similar to that in [5]. However, whereas the end effector in [5] tracks a 
prescribed end effector path, here only the final end effector position is required. The 
error vector e is defined: 


where r T e R } 
a fixed target 


is the vector locating the target with respect to the robot base frame. For 

oo 


The Lyapunov scalar function v is defined 

v = ie r e 00 

Because v is a positive scalar (related to the error's magnitude squared), then, if the time 
derivative of v is negative, e will go to zero as time approaches infinity. Taking the time 

derivative gives: 


v = e T e 

=-£ r Ui) 


(13) 


An obvious selection for q R is to make its elements proportional to the elements of J e . 
One such solution is given by 


where M is a positive definite matrix of dimension nxn. The computational simplicity 
of Eq. (14) contrasts starkly with the complexity of computing the motion using t e 

pseudo-inverse approach in Eq. (7). In addition, since q R is a unit vector scaled by M 
and no matrices must be inverted, the control law works well even in the vicinity of joint 
singularities. 

The importance of an appropriate metric must be emphasized. Doty, et al [9] 
show that results may be obtained which are non-invariant with respect to choice of 

m 

reference frame or dimensional units used to describe the problem. In Eq. (14) q R has the 

dimensions of radians/time for revolute joints and length/time for prismatic joints. On the 
right hand side of Eq. (14), J has dimensions of length/radians and length/length for 
revolute and prismatic joints, respectively, while the error vector has the units of length. 
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Clearly the units of J T e are incompatible with q R . Normalizing J T e as a unit vector 

renders it dimensionless and the matrix A/ serves to give the appropriate unit metric. In 
addition, the selection of the magnitude of the elements of M may be used to emphasize 
the motion of some joints over that of others. This aspect will be discussed later as a 
means of avoiding excessive joint rates or deflections. 

Figure 2 shows three simulations of robot motion for different values of a diagonal 
matrix M. For all the maneuvers, the manipulator has an initial end effector position 
r(0)=[3, 2, 0], shown in Fig. 2(a), and moves to a final end effector position of r(tj)= [-3, 
2, 0], indicated by the dot in the upper left portion of the workspace. In Fig. 2(b) the 
final configuration and the end effector path are shown for M equal to the identity matrix. 
That is, all of the joint rates are equally weighted. The end effector trajectory resembles a 
damped sinusoid. 

In Fig. 2(c) the motion of the last two revolute joints is given a weight of ten times 
greater than the other nine joints. This causes the manipulator to attempt to reach the 
target primarily by moving these two joints, which at one point causes a near singularity. 
This is evidenced by the abrupt direction change of the end effector. Because of a fairly 
large step size in the Mathematica program, the final conditions are not satisfied exactly. 

In Fig. 2(d) the motion of the translating base and the first revolute joint are 
emphasized by a factor of ten. The motion to the target is accomplished almost 
exclusively by the motion of these two joints. 


End Effector Trajectory Determined By Lyapunov Function 

(a) Initial Configuration 

(b) Final Configuration with Equal Weights on Joint Rates 

(c) Final Configuration with Last Two Joint Rates Emphasized 

(d) Final Configuration with First Two Joint Rates Emphasized 
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V. OBSTACLE AVOIDANCE 


As noted before, most other proposed methods of obstacle avoidance presume a 
prescribed end effector path that is obstacle free. This can be an important constraint 
because it may require detailed knowledge of the work space or excessively complicated 
path planing. By allowing the effector to seek its own path, the overall manipulator 
configuration becomes much more robust in its ability to avoid obstacles. 

As suggested by Khatib[10], each obstacle is assigned a cost function. Figure 3 
shows a representative manipulator arm with obstacle avoidance points p , i = 1,... rip, 

where y, z,J, identified along it. A likely location for such points would be 

the manipulator joints and the link mid points but they may be dictated by sensor location 
or other criteria. In this paper, obstacles are assumed to be rectangular parallelepipeds 
with their center points oj , and with dimensions 2aj, 2b j, 2 cj,j- 1 ,...n Q . 



Figure 3. 

Typical Designated Obstacle Avoidance Points 

The cost function for the _/'//, obstacle with respect to the iff, obstacle avoidance 
point is given by the super-ellipsoid 




f \* 

' x. -x. ' 


a J ) 


+ 


/ V 

y,-y, 


v 


j 


fvVf 

, <7 J 


( 15 ) 


Contact with the surface of the obstacle by the iff, point is approximated by C ; (/? ) = 1. 
A potential function is defined by: 


os) 

which guarantees that the cost becomes infinite before actual contract is made with the 
obstacle. The gradient of the potential function is 



( 17 ) 
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The time rate of change of P can thus be expressed 

^ = - T - ^ 8) 
As one might expect, joint rates generated by Eq. (14) may also adversely change 

the pj^to Obstacles. However, if the only requirement on 

error vector be decreased over every sub-mterval, then an mfimte num ^ trajectory 

may be found which accomplish this. Assummg that at least one unobstru j ry 

exists, one possible solution is to find the component of q R from Eq. (14) which is 
orthogonal to M This results in >=0. The Gram-Schimdt procedure described by 
Luenberger [11], subtracts from q R its projection in the p direction. This results 
commanded joint rates: 


1 = 1r- 


1r e 


(19) 


where £ is a uni. vector parallel to ft. A three dimensional analogy is shown in Figure 4^ 
Moving toward a destination, a traveler's path intersects a portion ° f a . Th * 

the target is downhill. 

In the three dimensional analogous state, it is easy to see 
redundancy increases its robustness. 
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The control embodied in Eqs. (14) and (19) is demonstrated in the simulation 
shown in Figure 5. Because the interest in serpentine manipulators is largely due to the 
potential for reaching targets in constricted areas, in this example, the target is located 
"down the hall and around the comer" with the walls modeled by three obstacles. A 
collision free trajectory is generated by the method described above. Once again the end 
effector trajectory is non-intuitive. 


0 - 


•I.) 


{].». 0.22S. 0.231} 



{3. *7, t.3t. 0.131} 


(3.40. 2.41. -0.124} 



Figure 5 

Obstacle Avoidance Trajectory 
(a) Initial Configuration 
(b),(c) Intermediate Configurations 
(d) Final Configuration 
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VI. LIMITATIONS ON JOINT RATES AND DEFLECTIONS 

In addition to avoiding obstacles, manipulator arms are frequently limited in the 
magnitude of the joint deflections which can be achieved. In addition the joint rates 
usually limited by the manipulator architecture. While not explicitly examined 
author's research, some possible solutions are suggested. 

It has been demonstrated that the joint rates can be influenced by the weighting of 
the elements of the matrix M. The most straightforward approach is to weight each jo 
fate "dent, y by making M diagonal. The weigh, on each jom. rate may be made a 

function of its current deflection and commanded joint rate. 

It is useful to think of the diagonal elements of Mas the stiffness coefficients of n 
non-linear spring. The deflection of the i th joint is bounded by q^q^q L- 
Defining. 


*7' max r 

r 1 = ? im « + ^r 


f 




Vi = sign[(j r e).] 


( 20 ) 


The diagonal elements ofMare defined: 

m u = k,(l- Tlif,), f " 1 .— 


( 21 ) 


where k- t <q^l 2. 

This function guarantees that the maximum allowable joint rate is never exceeded 
the examples in this paper and requires further verification. 
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vn. CONCLUSIONS AND RECOMMENDATIONS 

A method for moving a serpentine manipulator's end effector to a target in a 
constricted area while avoiding collision's of the manipulator's arm with the surrounding 
workspace has been demonstrated. This method has the virtues of being computationally 
straightforward. It is robust in the vicinity of singularities and multiple obstacles. While 
it must be emphasized that this paper discusses only very preliminary results, the algorithm 
appears to have great potential for successful implementation for achieving numerous 
robot tasks. 

Although the algorithm appears to be fairly versatile, the ability of the manipulator 
to reach a target can be sensitive to its initial configuration relative to the target. While 
path planning is not explicitly required, it is necessary to orient the robot with respect to 
the target so that a likely path is unambiguous. In addition, it has been observed that there 
are a number of cases in which the robot will not be able to reach the target. It is possible 
for the end effector to arrive a point where no further forward motion is possible. This is 
the case where a wide, flat obstacle is approached and only moving away from the target 
will eventually result in a configuration in which forward motion. Additional heuristics 
need to be developed to address this possibility. 

The scaling matrix requires further research. While the suggested configuration 
works adequately, there is room for improvement. Further research is especially 
necessary in properly scaling the control vector in order to avoid joint rate and deflection 
limitations. These limitations, although addressed in this paper, should be further 
investigated in the context of a realistic robot architecture. 

Finally, although Mathematica is a versatile programming tool, it is far too slow 
for practical numerical integration. For implementation on an actual robot, programming 
in C language is recommended. Mathematica may be linked to a C code to evaluate 
symbolically complicated expressions such as the Jacobian matrix or the obstacle gradient 
vector. 
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VDL APPENDIX: MATHEMATICA PROGRAM FOR ROBOT SIMULATION 

Mathematica was used to produce the simulations in this paper. The parameters in the 
following program are those for the simulation depicted in Fig. 5. Mathematica 
commands are shown in Courier font. Explanatory comments have been added in 
Times Roman font. 


ClearAll [x, theta, al, p, rO, J, TO] ; 

Clear [n, d, obsnum] ; 

JacobianMatrix [ funs_List, vars_List] :=Outer [D, funs, vars] ; 
Norm [ vars_List ] :=Sqrt [vars .vars] ; 

UnitVector [vars_List] :=vars/Sqrt [vars .vars] ; 
al:=Array [alpha, n] ; (*Vector of frame rotation angles*) 
x:=Append [Array [theta, n] , d] ; (*Generalized Vector ofjoint displacements. 
The translational displacement of the base is given by d.*) 
p : =Array [ 1 , n] ; (^Vector of link lengths*) 


(*R[n] defines the rotation matrix relating the nth link frame to the {n-\)th frame using 
standard Denavitt-Hartenberg convention *) 


R[n_] :={ [Cos [x [ [n] ] ] , -Cos [al [ [n] ] ] Sin[x[ [n] ] ] , Sin [al [ [n] ] ] 

Sin [x [ [n] ] ] } , 

{ Sin [x [ [n] ] ] ,Cos [al [ [n] ] ] 

Cos [x [ [n] ] ] } , 

{ 0, Sin [al [ [n] ] ] , Cos [al [ [n] ] ] } } ; 


Cos [x [ [n] ] ] , -Sin [al [ [n] ] 


(*T0[n] gives the orientation of the nth link with respect to the base frame*) 

T0[n_] : =T0 [n] =T0 [n-1 ] .R[n] ; 

TO [0] =IdentityMatrix [3] ; 

R [ 0 ] =Ident ityMatrix [ 3 ] ; 

r0[n_] :=rO[n]=r0[n-l]+T0[n] . {p [ [n] ] , 0, 0 } ; (*Endpoint of nth link*) 

mp[n_] : =mp [n] =r0 [n-1 ] +T0 [n] . [p[ [n] ] /2, 0, 0 } ;(*Midpoint otnth link*) 
rO [0] = {d, 0,0}; 

mp [ 0 ] = { d, 0 , 0 } ; 

(*Manipulator architecture is defined by n, al, and p *) 

ali{ Pi/2, -Pi/2, Pi/2, -Pi/2, Pi/2, -Pi/2, Pi/2, -Pi/2, Pi/2, -Pi/2 } ; 
p= { 0, 1, 0, 1, 0, 1, 0, 1,0,1}; 

joints=Table [Point [rO [i] ],{ i, 0, n} ] ; (*Table of joint coordinates, for 
plotting purposes*) 

arm=Line [Table [rO [i] , {i,0,n}] ] ;(Line from joint to joint; for plotting 
purposes*) 

J=JacobianMatrix [ rO [n] , x] ; 
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(■"Obstacles are depicted as rectangular solids with six coordinates: first three values are 
coodinates of mass center. Second three values are x,y,z dimensions*) 
obsnum=3 ; 

ob[l]={2,2,-.5,2,3,5}; 
ob[2]={2.75,-.75,-.5,3.5,.5,5}; 
ob[3] = {4. 25, 1.25/-. 5, .5/3.5/51; 

(*obstacleshape is a function which draws a rectangular solid in the plot to represent 
each obstacle*) 
obstacleshape [k_] := 

Cuboid [ { (ob [k] ( [ 1 ] ] -ob [k] [[4]]/2), 

(ob[k] [ [2] ]-ob[k] ( [5] ]/2) , (ob[k] [ [3]] -ob [k] [ [6] ] /2) } , 

{ ( ob [ k ] [ [1] ]+ob[k] [ (4 ] ] / 2 ) , 

(ob [ k] [ [2 ] ] +ob [k] ( [5] ] /2 } , (ob[k] [[3]]+ob[k] [ [ 6] ] /2) }]; 

(*cost is the potential function*) 
cost= (Suni[Sum[ 

1/ (Sum [ ( (mp [ i ] [ [ j ] ] -ob [k] [ [ j ] ] ) / (ob[k] [ [ j + 3] ] /2) ) *8, 

{ j, 1,3} ]-l.l) , (i, 0,n}]+ 

1/ (Sum[ ( (rO [n] [ [ j ] ] - 

ob[k] [[j] ]) / (ob [kj [tj+3] ]/2)) A 8, {j, 1, 3}]— 1.1) , 

{ k, 1, obsnum} ] ) ; 

(* mu is the obstacle gradient vector*) 
mu=Table [D [cost, x [ [i] ] ] , { i, 1 , n+1 } ] ; 

step=. 05;(*step size*) 

imax=100 ; (*maximum number of steps*) 

targe t= (3.5,2.5,01; 

d=0 ; 

theta [ 1 ] =0; 

theta [2 ] =N[Pi/2] ; 

theta [ 3 ] =0 ; 

theta [ 4 ] =-N [Pi/2 ] ; 

theta [ 5 ] = 0 ; 

theta [ 6] =-N [Pi/2] ; 

theta [7] =0; 

theta [8 j=0; 

theta [ 9] =0 ; 

theta [ 10 ] =0; 

pts=[Point [rO [n] ] } ; 

base={ Point [ { d, 0 , — 3 } ] }; 

Metric=DiagonalMatrix [1,1,1,1,1,1,1,1,1/1,13; 
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(* Robotsim is a function which performs the actual simulation. It is largely devoted to 
drawing the graphic images of the robot motion. It is started by compiling the program 

and then typing "Robotsim"*) 

Robotsim: = 

For [i=0, i<=imax, i++, . 

If [EvenQ[i] , path=Append[pts, Point [rO [n] J J , 
track=Append [base, Point [ {d, 0, -3} ] ] ; 

Show [ 

Graphics3D[{ {AbsoluteThickness [2] , arm. 

Cuboid [{d- -2, -.2,-3} / { d+ . 2 , .2,0}]}, 

{RGBColor [1,0,0] , PointSize [. 02] joints , 

{ RGBColor[0,0,l],AbsoluteThickness 1 /Table pathU, 

{RGBColor [1,1,0], AbsoluteThickness [ 1] , Table [track] 

{RGBColor [0, 1, 0] , PointSize [. 02] , Point [target] }, 

{Table [obstacleshape [i] , {i, 1, obsnum} J } ' 

1,5}, {-1,5}, {-3,5}}] ; 

pts=path; 

base=track; 

Tf}sgrt [1 target-r0 [n] ) . (target-rO [n] ) ] < . 001, i=imax ] ; 

(* The last six lines of code perform a fairly crude numerical integration with a first order 

Euler’s method*) _ w . . . 

vr=UnitVector [ (target-rO [n] ) .J. Metric] , 

nu=UnitVector [mu] ; 
v=UnitVector [vr- vr.nu nu] ; 

new[ j ] =theta [ j ] +step v[ [j] ] ; theta [j ] =new[j ] ] , 
new [n+l]=d+ step v[ [n+1] ] ;d=new[n+l] ] ; 
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